#include "morphing_algo.h"
#include <TriMesh.h>
#include <iostream>
#include <debug.h>
#include <MatlabSolver.h>


using namespace CGF;
morphing_algo::morphing_algo(TriMesh *s , TriMesh *t):sourceMesh(s),targetMesh(t),interpolationMeshes(0)
{

	S = NULL;
	R= NULL;
	Ainv = NULL;
	nv = sourceMesh->vertices.size();
	nf = sourceMesh->faces.size();
	log = new Logger;
}

morphing_algo::~morphing_algo(void)
{
	for(int i = 0; i <interpolationMeshes.size(); i++)
		delete interpolationMeshes[i];
	if(S)
		delete [] S;
	if(R)
		delete []R;
	
	if(Ainv)
		delete Ainv;

	delete log;

}

void morphing_algo::compute_interpolation_mesh(float time )
{
	//At
	Eigen::Matrix2d *At = new Eigen::Matrix2d[nf];
	
	Eigen::Matrix2d I = Eigen::Matrix2d::Zero();
	I(0,0) = I(1,1) = 1;

	//	compute At
	for(int i = 0; i < nf;i++)
	{
		At[i] = R[i] * ( (1-time)*I + time * S[i]);
	}

	//solve v
	int addf = 1;

	Eigen::VectorXd s_i((4*nf)*6+2*addf);
	Eigen::VectorXd s_j((4*nf)*6+2*addf);
	Eigen::VectorXd s_val((4*nf)*6+2*addf);
	Eigen::VectorXd b(2*addf+4*nf);

	int row = 1;
	int idx= 0;
	for(int i = 0; i < nf; i++)
	{
		int v[3];
		for(int j = 0; j < 3; j++)
			v[j] = sourceMesh->faces[i][j]*2+1;
		
		for(int j=0; j < 2; j++)
		{
			for(int k = 0; k <2; k++)
			{
				int r= j *2 +k;
				for(int h = 0; h<3; h++)
				{
					s_i(idx) = row;
					s_j(idx) = v[h];
					s_val(idx++) = Ainv[i](r,2*h);

					s_i(idx) = row;
					s_j(idx) = v[h]+1;
					s_val(idx++) = Ainv[i](r,1+2*h);
				}
		
				b(row-1) = At[i](j,k);
				row++;
			}
		}	
	}
	
	//fixed point
	point sp0 =  sourceMesh->vertices[0];
	point tp0 =  targetMesh->vertices[0];
	point pFixed =(tp0-sp0) * time + sp0;
	
		s_i(idx) = row;
		s_j(idx) = 1;
		s_val(idx++) = 1;
		b(row-1)  = pFixed[0];

		s_i(idx) = row+1;
		s_j(idx) = 2;
		s_val(idx++) = 1;
		b(row) = pFixed[1];

	CGF::MatlabSparseSolver solver;
	 solver.connect_matlab();

	 Eigen::VectorXd x(2*nv);
	 dbg(-1)<< solver.solve(s_i, s_j, s_val, b, x);

	{   
		double error = 0;
		for(int i = 0; i < nv; i++)
		{
			error += (targetMesh->vertices[i][0] - x(i*2)) *(targetMesh->vertices[i][0] - x(i*2)) +
				(targetMesh->vertices[i][1]  -  x(i*2+1)) *
				(targetMesh->vertices[i][1]  -  x(i*2+1));

			sourceMesh->vertices[i][0] = x(i*2);
			sourceMesh->vertices[i][1] = x(i*2+1);
		}
		std::cout <<"error "<<error<<std::endl;
	}
	
	delete [] At;
}

void morphing_algo::precompute_R_S()
{
	Eigen::Matrix2d A;

	R = new Eigen::Matrix2d[nf];
	S = new Eigen::Matrix2d[nf];
	Ainv = new Matrix6d[nf];

   Eigen::Vector2d l;
	//coupute R,S,L
   CGF::MatlabTool matlabTool;
   if(! matlabTool.connect_matlab())
	   return;

	 Eigen::Matrix2d U;
	 Eigen::Matrix2d s;
	 Eigen::Matrix2d V;
	
	for(int i = 0; i < nf; i++)
	{
		computeA(i,i, A, Ainv[i]);

	    matlabTool.set_variable("A",A);
       
		matlabTool.exe_command("[U,S,V]=svd(A);V=V';");//
	
		 matlabTool.get_variable("U",U);
		 matlabTool.get_variable("S",s);
		 matlabTool.get_variable("V",V);
		 
		 Eigen::Matrix2d a = A- U*s*V;

		R[i] = U*V;
		S[i] = V.transpose() *s * V;
	}
}
void morphing_algo::computeA(const int &sf, const int &tf, Eigen::Matrix2d &A, Matrix6d& Ainv)
{
	Matrix6d Ax =Matrix6d::Zero();
	Vector6d b = Vector6d::Zero();

	for(int i = 0; i < 3; i++)
	{
		point p = sourceMesh->vertices[sourceMesh->faces[sf][i]];
		point q = targetMesh->vertices[targetMesh->faces[tf][i]];
	
		int row = i *2;

		Ax(row,0) = p[0];
		Ax(row,1) = p[1];
		Ax(row,4) = 1;
		b(row) = q[0];
		row++;
		Ax(row,2) = p[0];
		Ax(row,3) = p[1];
		Ax(row,5) = 1;
		b(row) = q[1];
	}

     Ainv = Ax.inverse();
	 Vector6d x = Ax.colPivHouseholderQr().solve(b);
	 A(0,0) = x(0);
	 A(0,1) = x(1);
	 A(1,0) = x(2);
	 A(1,1) = x(3);

}



